
#ifndef MATRIX2_H_
#define MATRIX2_H_

#include "Vector2.h"

namespace p2 {

// Rotation matrices are of the form
//   R = cos(t) -sin(t)
//       sin(t)  cos(t)
// where t > 0 indicates a counterclockwise rotation in the xy-plane.

template<typename Real>
class Matrix2
{
public:
	Matrix2();	// uninitialised
	Matrix2(const Matrix2& mat);
	Matrix2(Real m00, Real m01, Real m10, Real m11);
	Matrix2(Real const entry[4], bool rowMajor);
	Matrix2(Vector2<Real> const& u, const Vector2<Real>& v, bool columns);
	Matrix2(Vector2<Real> const* vectors, bool columns);
	Matrix2(Real m00, Real m11);
	Matrix2(Real angle);
	// create a tensor product U*V^T.
	Matrix2(Vector2<Real> const& u, Vector2<Real> const& v);

	// Assignment.
	Matrix2& operator=(Matrix2 const& mat);

	// Create various matrices.
	void makeZero();
	void makeIdentity();
	void makeDiagonal(Real m00);
	void makeDiagonal(Real m00, Real m11);
	void makeRotation(Real angle);
	void makeTensorProduct(Vector2<Real> const& u, const Vector2<Real>& v);

	static inline Matrix2<Real> cross(Vector2<Real> const& r);

	/*
	 * operations
	 */
	Matrix2 operator+(Matrix2 const& mat) const;
	Matrix2 operator-(Matrix2 const& mat) const;
	Matrix2 operator*(Real scalar) const;
	Matrix2 operator/(Real scalar) const;
	Matrix2 operator-() const;
	Vector2<Real> operator*(Vector2<Real> const& vec) const;
	Matrix2 operator*(Matrix2 const& mat) const;
	Matrix2& operator+=(Matrix2 const& mat);
	Matrix2& operator-=(Matrix2 const& mat);
	Matrix2& operator*=(Real scalar);
	Matrix2& operator/=(Real scalar);
	Real const* operator[](int row) const;
	Real* operator[](int row);

	Matrix2 transpose() const;
	Matrix2 inverse(Real const epsilon) const;
	Matrix2 adjoint() const;
	Real determinant() const;
	void extractAngle(Real& angle) const;
	/*
	 * pre-defined
	 */
	static const Matrix2 ZERO;
	static const Matrix2 IDENTITY;

protected:
	Real m_t[4];
};

// c * M
template<typename Real>
inline Matrix2<Real> operator*(Real scalar, Matrix2<Real> const& mat);

// v^T * M
template<typename Real>
inline Vector2<Real> operator*(Vector2<Real> const& vec, Matrix2<Real> const& mat);

template<typename Real>
Matrix2<Real>::Matrix2()
{
}

template<typename Real>
Matrix2<Real>::Matrix2(Matrix2 const& mat)
{
	m_t[0] = mat.m_t[0];
	m_t[1] = mat.m_t[1];
	m_t[2] = mat.m_t[2];
	m_t[3] = mat.m_t[3];
}

template<typename Real>
Matrix2<Real>::Matrix2(Real m00, Real m01, Real m10, Real m11)
{
	m_t[0] = m00;
	m_t[1] = m01;
	m_t[2] = m10;
	m_t[3] = m11;
}

template<typename Real>
Matrix2<Real>::Matrix2(Real const entry[4], bool rowMajor)
{
	if (rowMajor) {
		m_t[0] = entry[0];
		m_t[1] = entry[1];
		m_t[2] = entry[2];
		m_t[3] = entry[3];
	} else {
		m_t[0] = entry[0];
		m_t[1] = entry[2];
		m_t[2] = entry[1];
		m_t[3] = entry[3];
	}
}

template<typename Real>
Matrix2<Real>::Matrix2(Vector2<Real> const& u, Vector2<Real> const& v, bool columns)
{
	if (columns) {
		m_t[0] = u[0];
		m_t[1] = v[0];
		m_t[2] = u[1];
		m_t[3] = v[1];
	} else {
		m_t[0] = u[0];
		m_t[1] = u[1];
		m_t[2] = v[0];
		m_t[3] = v[1];
	}
}

template<typename Real>
Matrix2<Real>::Matrix2(Vector2<Real> const* v, bool columns)
{
	if (columns) {
		m_t[0] = v[0][0];
		m_t[1] = v[1][0];
		m_t[2] = v[0][1];
		m_t[3] = v[1][1];
	} else {
		m_t[0] = v[0][0];
		m_t[1] = v[0][1];
		m_t[2] = v[1][0];
		m_t[3] = v[1][1];
	}
}

template<typename Real>
Matrix2<Real>::Matrix2(Real m00, Real m11)
{
	makeDiagonal(m00, m11);
}

template<typename Real>
Matrix2<Real>::Matrix2(Real angle)
{
	makeRotation(angle);
}

template<typename Real>
Matrix2<Real>::Matrix2(Vector2<Real> const& u, Vector2<Real> const& v)
{
	makeTensorProduct(u, v);
}

template<typename Real>
Matrix2<Real>& Matrix2<Real>::operator=(Matrix2 const& mat)
{
	m_t[0] = mat.m_t[0];
	m_t[1] = mat.m_t[1];
	m_t[2] = mat.m_t[2];
	m_t[3] = mat.m_t[3];
	return *this;
}

template<typename Real>
void Matrix2<Real>::makeZero()
{
	m_t[0] = (Real) 0;
	m_t[1] = (Real) 0;
	m_t[2] = (Real) 0;
	m_t[3] = (Real) 0;
}

template<typename Real>
void Matrix2<Real>::makeIdentity()
{
	m_t[0] = (Real) 1;
	m_t[1] = (Real) 0;
	m_t[2] = (Real) 0;
	m_t[3] = (Real) 1;
}

template<typename Real>
void Matrix2<Real>::makeDiagonal(Real m00)
{
	m_t[0] = m00;
	m_t[1] = (Real) 0;
	m_t[2] = (Real) 0;
	m_t[3] = m00;
}

template<typename Real>
void Matrix2<Real>::makeDiagonal(Real m00, Real m11)
{
	m_t[0] = m00;
	m_t[1] = (Real) 0;
	m_t[2] = (Real) 0;
	m_t[3] = m11;
}

template<typename Real>
void Matrix2<Real>::makeRotation(Real angle)
{
	m_t[0] = Math<Real>::Cos(angle);
	m_t[2] = Math<Real>::Sin(angle);
	m_t[1] = -m_t[2];
	m_t[3] = m_t[0];
}

template<typename Real>
void Matrix2<Real>::makeTensorProduct(Vector2<Real> const& u, Vector2<Real> const& v)
{
	m_t[0] = u[0] * v[0];
	m_t[1] = u[0] * v[1];
	m_t[2] = u[1] * v[0];
	m_t[3] = u[1] * v[1];
}

template<typename Real>
inline Matrix2<Real> Matrix2<Real>::cross(Vector2<Real> const& r)
{
	real rxy = r[0] * r[1];
	real rxx = r[0] * r[0];
	real ryy = r[1] * r[1];
	return Matrix2<Real>(+rxx, -rxy, -rxy, +ryy);
}

template<typename Real>
Matrix2<Real> Matrix2<Real>::operator+(Matrix2 const& mat) const
{
	return Matrix2<Real>(m_t[0] + mat.m_t[0], m_t[1] + mat.m_t[1], m_t[2] + mat.m_t[2],
		m_t[3] + mat.m_t[3]);
}

template<typename Real>
Matrix2<Real> Matrix2<Real>::operator-(Matrix2 const& mat) const
{
	return Matrix2<Real>(m_t[0] - mat.m_t[0], m_t[1] - mat.m_t[1], m_t[2] - mat.m_t[2],
		m_t[3] - mat.m_t[3]);
}

template<typename Real>
Matrix2<Real> Matrix2<Real>::operator*(Real scalar) const
{
	return Matrix2<Real>(scalar * m_t[0], scalar * m_t[1], scalar * m_t[2], scalar * m_t[3]);
}

template<typename Real>
Matrix2<Real> Matrix2<Real>::operator/(Real scalar) const
{
	if (scalar != (Real) 0) {
		Real invScalar = ((Real) 1) / scalar;
		return Matrix2<Real>(invScalar * m_t[0], invScalar * m_t[1], invScalar * m_t[2],
			invScalar * m_t[3]);
	} else {
		return Matrix2<Real>(Math<Real>::MAX_REAL, Math<Real>::MAX_REAL,
			Math<Real>::MAX_REAL, Math<Real>::MAX_REAL);
	}
}

template<typename Real>
Matrix2<Real> Matrix2<Real>::operator-() const
{
	return Matrix2<Real>(-m_t[0], -m_t[1], -m_t[2], -m_t[3]);
}

template<typename Real>
Matrix2<Real>& Matrix2<Real>::operator+=(const Matrix2& mat)
{
	m_t[0] += mat.m_t[0];
	m_t[1] += mat.m_t[1];
	m_t[2] += mat.m_t[2];
	m_t[3] += mat.m_t[3];
	return *this;
}

template<typename Real>
Matrix2<Real>& Matrix2<Real>::operator-=(Matrix2 const& mat)
{
	m_t[0] -= mat.m_t[0];
	m_t[1] -= mat.m_t[1];
	m_t[2] -= mat.m_t[2];
	m_t[3] -= mat.m_t[3];
	return *this;
}

template<typename Real>
Matrix2<Real>& Matrix2<Real>::operator*=(Real scalar)
{
	m_t[0] *= scalar;
	m_t[1] *= scalar;
	m_t[2] *= scalar;
	m_t[3] *= scalar;
	return *this;
}

template<typename Real>
Matrix2<Real>& Matrix2<Real>::operator/=(Real scalar)
{
	if (scalar != (Real) 0) {
		Real invScalar = ((Real) 1) / scalar;
		m_t[0] *= invScalar;
		m_t[1] *= invScalar;
		m_t[2] *= invScalar;
		m_t[3] *= invScalar;
	} else {
		m_t[0] = Math<Real>::MAX_REAL;
		m_t[1] = Math<Real>::MAX_REAL;
		m_t[2] = Math<Real>::MAX_REAL;
		m_t[3] = Math<Real>::MAX_REAL;
	}

	return *this;
}

template<typename Real>
Vector2<Real> Matrix2<Real>::operator*(Vector2<Real> const& vec) const
{
	return Vector2<Real>(
		m_t[0] * vec[0] + m_t[1] * vec[1],
		m_t[2] * vec[0] + m_t[3] * vec[1]
	);
}
template<typename Real>
Real const* Matrix2<Real>::operator[](int row) const
{
	return &m_t[2 * row];
}
template<typename Real>
Real* Matrix2<Real>::operator[](int row)
{
	return &m_t[2 * row];
}

template<typename Real>
Matrix2<Real> Matrix2<Real>::transpose() const
{
	return Matrix2<Real>(m_t[0], m_t[2], m_t[1], m_t[3]);
}

template<typename Real>
Matrix2<Real> Matrix2<Real>::operator*(Matrix2 const& mat) const
{
	// A*B
	return Matrix2<Real>(
		m_t[0] * mat.m_t[0] + m_t[1] * mat.m_t[2],
		m_t[0] * mat.m_t[1] + m_t[1] * mat.m_t[3],
		m_t[2] * mat.m_t[0] + m_t[3] * mat.m_t[2],
		m_t[2] * mat.m_t[1] + m_t[3] * mat.m_t[3]);
}

template<typename Real>
Matrix2<Real> Matrix2<Real>::inverse(Real const epsilon) const
{
	Matrix2<Real> inverse;

	Real det = m_t[0] * m_t[3] - m_t[1] * m_t[2];
	if (Math<Real>::FAbs(det) > epsilon) {
		Real invDet = ((Real) 1) / det;
		inverse.m_t[0] = m_t[3] * invDet;
		inverse.m_t[1] = -m_t[1] * invDet;
		inverse.m_t[2] = -m_t[2] * invDet;
		inverse.m_t[3] = m_t[0] * invDet;
	} else {
		inverse.m_t[0] = (Real) 0;
		inverse.m_t[1] = (Real) 0;
		inverse.m_t[2] = (Real) 0;
		inverse.m_t[3] = (Real) 0;
	}

	return inverse;
}

template<typename Real>
Matrix2<Real> Matrix2<Real>::adjoint() const
{
	return Matrix2<Real>(m_t[3], -m_t[1], -m_t[2], m_t[0]);
}

template<typename Real>
Real Matrix2<Real>::determinant() const
{
	return m_t[0] * m_t[3] - m_t[1] * m_t[2];
}

template<typename Real>
void Matrix2<Real>::extractAngle(Real& angle) const
{
	// assert:  'this' matrix represents a rotation
	angle = Math<Real>::ATan2(m_t[2], m_t[0]);
}

template<typename Real>
inline Matrix2<Real> operator*(Real scalar, Matrix2<Real> const& mat)
{
	return mat * scalar;
}

template<typename Real>
inline Vector2<Real> operator*(Vector2<Real> const& vec, Matrix2<Real> const& mat)
{
	return Vector2<Real>(
		vec[0] * mat[0][0] + vec[1] * mat[1][0],
		vec[0] * mat[0][1] + vec[1] * mat[1][1]
	);
}

}

#endif	//MATRIX2_H_
